/*
 * arrow.cpp
 *
 *  Created on: 31 mai 2012
 *      Author: turtlebot
 */

#include <stdio.h>
#include <math.h>
#include "cv.h"
#include "highgui.h"

IplImage* drawArrow2(double angle, double speed, IplImage* image, int width, int height)
{

	if(speed != 0.0){
		if(speed < 0){
			speed = speed * (-1);
			if(angle < 0){
				angle = -3.14-angle;
			}
			else{
				angle = 3.14-angle;
			}
		}

		// determination de la longueur selon la vitesse
		int length = (int)floor(speed*100);
		int length2 = (int)floor(speed*90);

		// origine de l'image (centre)
		CvPoint* origine = new CvPoint();
		origine->x = (int)width/2;
		origine->y = (int)height/2;


		// coordonnees cartesiennes de l'extremite deduites de ses coordonnees polaires (vitesse traduite en distance et angle en radians)
		int x = (int)floor(length*cos(angle));
		int y = (int)floor(length*sin(angle));

		// Adaptation des coordonnees au referentiel de l'image
		// Premiere extremite de la droite
		CvPoint* e1 = new CvPoint();
		e1->x = origine->x-y;
		e1->y = origine->y-x;

		// Seconde extremite de la droite
		//~ cvPoint e2 = cvPoint(200+y, 150+x);
		CvPoint* e2 = new CvPoint();
		e2->x = origine->x+y;
		e2->y = origine->y+x;

		// Tete de fleche
		// droite
		int x1 = (int)floor(length2*cos(angle-0.314));
		int y1 = (int)floor(length2*sin(angle-0.314));
		CvPoint* ed = new CvPoint();
		ed->x = origine->x-y1;
		ed->y = origine->y-x1;
		// gauche
		int x2 = (int)floor(length2*cos(angle+0.314));
		int y2 = (int)floor(length2*sin(angle+0.314));
		CvPoint* eg = new CvPoint();
		eg->x = origine->x-y2;
		eg->y = origine->y-x2;


		/* draw the main line */
		cvLine(image,                  /* the dest image */
				*origine,          	     /* start point */
				*e1,           		     /* end point */
				cvScalar(0, 0, 255, 0),      /* the color; red */
				5, 8, 0);                    /* thickness, line type, shift */

		/* draw the right arrow line */
		cvLine(image,                  /* the dest image */
				*e1,          	     	 /* start point */
				*ed,           		     /* end point */
				cvScalar(0, 0, 255, 0),      /* the color; red */
				5, 8, 0);                    /* thickness, line type, shift */

		/* draw the left arrow line */
		cvLine(image,                  /* the dest image */
				*e1,          	     	 /* start point */
				*eg,           		     /* end point */
				cvScalar(0, 0, 255, 0),      /* the color; red */
				5, 8, 0);                    /* thickness, line type, shift */

		/* draw the opposite line */
		cvLine(image,                  /* the dest image */
				*origine,          	     /* start point */
				*e2,           		     /* end point */
				cvScalar(0, 0, 255, 0),      /* the color; green */
				5, 8, 0);                    /* thickness, line type, shift */
	}
	else{

		// vitesse nulle => surplace => dessin d'un cercle

		/* draw a green circle */
		cvCircle(image,                       /* the dest image */
		         cvPoint((int)width/2, (int)height/2), 25,     /* center point and radius */
		         cvScalar(0, 255, 0, 0),    /* the color; red */
		         5, 8, 0);                  /* thickness, line type, shift */

	}

	return image;

}
